//---------------------------------------------------------------------------
#ifndef PC2CM5H
#define PC2CM5H
//---------------------------------------------------------------------------
/*
 -------------------------------------
 ---- SERIAL PROTOCOL DEFINITION -----
 ----  PDA <--> BioLoid Controller ---
 -------------------------------------

 This protocol is an adaption of that developed by Joerg Wolf for the
 University of Plymouth Robot Football team.

 The protocol now expects integer data to be converted into the text equivalent
 with hexadecimal encoding, for example, 0x1BC6 would be sent as "1BC6".  This
 has been done to get around the potential problem of a delimiter character
 (i.e. '[' or ']' being sent as data. The change also applies to the CRC at the
 emd of the packet.

 The packet has been extended to allow 3 integers (in the range 0-FFFF) to be
 sent in 1 packet.  This is to cater for sending IMU data (i.e. pitch, roll & yaw)
 in 1 packet, or to cater for sending data from 3 arm joints simulataneously.

 The two changes above result in a considerably longer packet.

 The protocol has also been extended with the addition of extra commands.

 By default the CM5 expects the serial link to be operated at 57600bps with
 8 data bits & no parity.

 The packet now looks as follows:

 Byte(s)0   1   2,3,4,5,    6,7,8,9,    10,11,12,13,    14,15,16,17,    18, 19
            C
        [   O   <P1....>    <P2....>    <P3.......>     <CRC......>     ]   NULL
            M

COM stands for "command"
Px stands for "Parameter"

*/

#define PACKET_LEN      19  //  Be sure to leave space for the NULL byte on the end of this!

#define STOP_NOW	    '0' //  Ask robot will freeze immediately (and probably fall over, might be better to use STOP_PAGE or STOP_POSE)
#define DO_PAGE		    '1' //  Play page. P1: Page Number
#define DO_POSE		    '2' //  Play pose. P1: Page Number, P2:  Pose Number
#define STOP_POSE	    '3' //  Stop at the end of the pose
#define STOP_PAGE	    '4' //  Stop at the end of the page
#define GET_VERSION		'5' //  Asks the CM5 to report its version number, responds with a 'v' packet
#define SERVO_TRIM	    '6' //  P1: Servo ID, P2: Trim
#define BUGGY_MOVE	    '7' //  Reserved
#define BALANCE		    '8' //  Reserved
#define TURN_SERVO	    '9' //  Reserved

#define REPORT_ARMS     'b' //  P1 == 1 to report once, P1 == 0 for continuous reporting
#define L_ARM_POS       'c' //  Left  arm position (& report), P1: Shoulder fore/aft, P2: Splay, P3:Elbow
#define R_ARM_POS       'd' //  Right arm position (& report), P1: Shoulder fore/aft, P2: Splay, P3:Elbow
#define VERSION         'v' //  Version number report, P1 major, P2 minor
#define SERVO_POSITION  'A' //  P1: ServoID, P2: Position (0-1023), P3: Torque (0-1023 or FFFF to leave unchanged)
#define SERVO_ROTATE    'B' //  P1: ServoID, P2: Speed
#define OMNI_MODE       'C' //  Indicates that robot is to use an omni-wheel architecture. Parameters set ServoIDs of wheels
#define OMNI_MOVE       'D' //  Move omni-direction robot. Parameters set corresponding wheel rotations
#define IMU_INFO		'E'	//	P1: Pitch, P2: Roll
#define HIP_TRIM        'F' //  P1: Trim. +ve moves forward, -ve moves back
#define HIGH_LEVEL_GAIT 'H' //  Indicates that the robot will use high level gait. 
							//  Format:  Forward Value (between -15 to 15) which controls where in the dynamic gait parameter
							//  space the robot is operating.  
							//  Turn: The amount of plane  (This is just the same as the Turn gait parameter)  between -10 to 10
#define REPORT_IMU      'I' //  P1: ID
							//

#define REPORT_SERVO	'J' //  P1: Servo to report    P2:  2 Position 3 Velocity 5 Load  (Position and Velocity would be 6 etc)
#define READ_ALL_SERVOS 'K' //  No Parameters Have all servos report their values.  Pose the robot then call this to dump out the servo angles.
#define REPORT_FOOT		'L' //  P1: Foot Sensor to report  Have the foot sensor report one set of values
#define SET_GAIT_PARAM	'M'	//	P1:	Parameter:	0:	StrideLength,	P2: Parameter value
							//					1:	StrideHeight
							//					2:	Swing			Hips from side to side
							//					3:	Stride X Offset	Move centre of gait elispe; replaced, see below
							//						Stride X Offset is now used to control side steps.
							//						Set - to move left, + to move right.  10 average move, not more than 20.  Tested with step height of 3							//					4:	Stride Y Offset
							//					5:	Ticks Per Pose	
							//					6:	Tilt
							//					7:	Turn			-10: Right, 10: Left
							//					8:  NumPoses (This effects the frequency of the gait)
							//					9:  Camber Changes camber angle of the robot.
							//					10:  Sidestep variable may be redundant re notes about Stride X offset above.

#define REPORT_FOOT_CONT'N' //  P1: Foot Sensor Address.  Have the foot sensor report continuously using code in main. 
#define RELEASE_MOTORS  'O'   //  Release all motors by setting the Using Pose Control flag. 
#define DO_SIMPLE_POSE  'P'   //  Do Simple Pose from memory. Using the Custom_Poses memory as defined in BioloidBiped.h
#define REPORT_IMU_CONT 'Q' //  P1: IMU Sensor Address.  Have the IMU report continuously using code in main
#define WRITE_VAL 		'W' //  Write a value to a device (used for digital IO or ???) 

#define PAGE_WALK_FORWARDS      0
#define PAGE_WALK_BACKWARDS     0
#define PAGE_ROTATE_CLOCK       0
#define PAGE_ROTATE_ANTI        0
#define PAGE_STEP_LEFT          0
#define PAGE_STEP_RIGHT         0

/** @brief crc-ccitt truncated polynomial. */
#define POLY 0x1021
/** @brief crc-ccitt initial value. */
#define INITIAL_VALUE 0xFFFF

int StringIsCompletePacket(char *Packet);
int CheckPacket(char *Packet);
int CheckPacketCRC(char *Packet);
void MakePacket1(char *Packet, char Command, char *Data);
    //  Data must contain 12 characters.  NULL is optional
void MakePacket2(char *Packet, char Command, int i1, int i2, int i3);
void SplitPacket1(char *Packet, char *Data);
    //  Data will be 12 chars followed by a NULL
void SplitPacket2(char *Packet, int *i1, int *i2, int *i3);

int  HexCharToInt(char c);
    //  Pass in e.g. '3' or 'c' or 'C' & get back e.g. 0x3 or 0xC
int  HexCharsToInt(char *c, int CharsToConvert);
    //  e.g. HexCharsToInt("04fC", 4) returns 0x4FC.  CharsToConvert will normally be 2 or 4
void IntToHexChars(int Val, char *String, int ConvChars);
    //  e.g. IntToHexChars(0x12b, String, 4) yields String = "012B"
int crc_compute(char *buf, int size);

#endif
